#include <avr/interrupt.h>
#include <avr/io.h>
#include "common.h"
#include "app.h"
#include "led.h"
#include "radio\radio.h"
#include "uart\uart.h"

volatile unsigned int pulse_width=0;

#define IS_PIND1_HIGH() 		(PORTD|= (_BV(PIND1)))

#define  duty_opened	1100 //2000
#define  duty_closed	2000 //1100
#define  radio_mode 	1	// 1 - Receiver, 0 - Sender

void init_global(){
	
    /** Disable default prescaler to make processor speed 8 MHz. */
    CLKPR = (1<<CLKPCE);
    CLKPR = 0x00;	


	//BEGIN Servo Motor
	DDRC |= (1<<PORTC5); //Enable output for PSWM

	TCCR3B = (0<<CS30) | (1<<CS31) | (0<<CS32) | (1<<WGM33) | (1<<WGM32); 
	TCCR3A = (0<<WGM30) | (1<<WGM31) | (1<<COM3B1) | (0<<COM3B0);

	ICR3 = 40000; // The max width for the PWM (TOP)
	OCR3B = duty_closed;
	//END Servo Motor

	//BEGIN IR-SENSOR
	PORTE |= (PORTE1 | PORTE3);
	//END IR-SENSOR

	//BEGIN LED
	init_LED_D2();
	init_LED_D5();
	//END LED


	//BEGIN RADIO
	//radio_init((uint8_t) 110, 0xABBA, radio_mode);
	radio_init((uint8_t) 110, 0xABBA, radio_mode);
	radio_print("Connected");
	//END RADIO

	//BEGIN UART
		uart_init();
		uart_write("Connected\r\n", 11);
	//END UART
	sei(); /** enable global interrupts **/

}

int main(){

	init_global();
	int i=1;
	//OCR3B = duty_opened;


	while(1){
		if (radio_mode==0){	//Sender
			switch(i){
				case 1:
					on_LED(LED_D5_GREEN);
					off_LED(LED_D5_RED);
					radio_print("<01>");				
					break;
				case 2:
					on_LED(LED_D5_RED);
					off_LED(LED_D5_GREEN);
					radio_print("<02>");				
					break;
				case 3:
					off_LED(LED_D5_RED);
					radio_print("<00>");				
					i=0;
					break;
			}
			i++;
			on_LED(LED_D2_GREEN);
			delayFor(2000);
			off_LED(LED_D2_GREEN);
		}
	}
	for(;;);
	return 1;

}

static uint8_t radio_buf[PAYLOAD_BYTES];

/**
 * Interrupt service routine for when data arrives at the radio (incoming).
 */
ISR (INT4_vect)
{
    int i;
    int still_good;
    int len;
	uint8_t* buf_ptr;
	uint8_t prevByte, curByte;


	PORTD ^= LED_D2_GREEN;


    still_good = 1;
    len = 0;
    buf_ptr = radio_buf;

	
    // Dump the radio to the buffer
    for(i=0; i < PAYLOAD_BYTES; ++i)
    {
		curByte= radio_get_byte();
        *buf_ptr = curByte;
        if(still_good && *buf_ptr == 0)
        {
            len = i;
            still_good = 0;
        }

		prevByte=curByte;
        ++buf_ptr; 
    }
    if(still_good) len = PAYLOAD_BYTES;

	if (len>3){
		if (radio_buf[0]=='<' && radio_buf[3]=='>' ){
			switch (radio_buf[2]){
				case '0':
					off_LED(LED_D5_RED | LED_D5_GREEN);
					break;
				case '1':
					off_LED(LED_D5_RED);
					on_LED(LED_D5_GREEN);
					break;
				case '2':
					off_LED(LED_D5_GREEN);
					on_LED(LED_D5_RED);
					break;	
			}
		}
	}

	off_LED(LED_D2_GREEN);

    // Write the string up to first nul
    uart_write(radio_buf, len);

}
